/*
 * Copyright (C) 2009 RobotCub Consortium
 * Author: Alessandro Scalzo alessandro.scalzo@iit.it
 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
 *
 */

/**
*
@ingroup icub_guis
\defgroup icub_iCubSkinGui iCubSkinGui

A gui to display the output of fingertip/skin tactile sensors.

\section intro_sec Description
The application is used to graphically display the output of the iCub fingertip/skin tactile sensors.
This application can be used in two different modalities, CAN mode and YARP mode.
If used in CAN mode, the application reads sensor data directly from the CAN bus.

If used in "YARP" mode,the application requires a yarp port connection to the iCubInterface output.

\section lib_sec Libraries
YARP libraries.

\section parameters_sec Parameters

Usage:

iCubSkinGui <--from filename.ini> <--useCan>

You can use the --from parameter (optional) to specify a .ini configuration file.

If no configuration file is specified, then $ICUB_ROOT/app/iCubSkinDemo/lefthand.ini is used.

You can use the --useCan parameter (optional) to ask the application to read sensor data directly from the CAN bus
(the CAN parameters must be specified in the configuration file). Otherwise the application opens an input
yarp ports and waits for a yarp connection with a stream of sensory data.

\section portsa_sec Ports Accessed
None

\section portsc_sec Ports Created
This application can be used in two different modalities, CAN mode and YARP mode.

If used in CAN mode, this application does not create any input port, and the data are directly obtained from the CAN bus.

If used in YARP mode, this application creates the port:

/skinGui/<robotPart>:i

where <robotPart> correspondes to the value
of the parameter robotPart contained in the .ini configuration file.

For example, if $ICUB_ROOT/app/iCubSkinDemo/lefthand.ini
is used, the module creates the port /skinGui/left_hand:i. The port must be afterwards
connected to the output port corresponding to the skin output in iCubInterface.

Examples:

yarp connect /icub/skin/lefthand /skinGui/left_hand:i

yarp connect /icub/skin/righthand /skinGui/right_hand:i


\section in_files_sec Input Data Files
No input files are required by this application.

\section out_data_sec Output Data Files
No output files are generated by this application.

\section conf_file_sec Configuration Files
The application uses configuration files (e.g. lefthand.ini) located in $ICUB_ROOT/app/iCubSkinDemo.
You can select the configuration file used by the application using:

--from <filename>.

If the --from parameter is not specified, $ICUB_ROOT/app/iCubSkinDemo/lefthand.ini is used as default.

\section tested_os_sec Tested OS
Linux and Windows.

\section example_sec Example Instantiation of the Module

iCubSkinGui

iCubSkinGui --from lefthand.ini

iCubSkinGui --from righthand.ini

iCubSkinGui --from lefthand.ini --useCan

iCubSkinGui --from righthand.ini --useCan

\author Alessandro Scalzo, Lorenzo Natale, Marco Randazzo

Copyright (C) 2009 RobotCub Consortium

CopyPolicy: Released under the terms of the GNU GPL v2.0.

This file can be edited at /src/gui/iCubSkinGui/src/main.cpp.
**/

#include "qticubskinguiplugin.h"
#include <QDebug>

using namespace yarp::os;


QtICubSkinGuiPlugin::QtICubSkinGuiPlugin(QQuickItem *parent):
    QQuickPaintedItem(parent)
{
    // By default, QQuickItem does not draw anything. If you subclass
    // QQuickItem to create a visual item, you will need to uncomment the
    // following line and re-implement updatePaintNode()

     setFlag(ItemHasContents, true);

     gXpos = 32;
     gYpos = 32;

     gWidth = 300;
     gHeight = 300;
     gRowStride = 0;
     gImageArea = 0;
     gImageSize = 0;
     gMapSize = 0;

     gpActivationMap = NULL;
     gpImageBuff = NULL;
     gpSkinMeshThreadCan = NULL;
     gpSkinMeshThreadPort = NULL;

     window_title = "QtICubSkinGui";


     TheadType = TYPE_PORT;
     timer.setInterval(50);
     timer.setSingleShot(false);
     connect(&timer,SIGNAL(timeout()),this,SLOT(onTimeout()));

}

QtICubSkinGuiPlugin::~QtICubSkinGuiPlugin()
{
    timer.stop();
    mutex.lock();

    if(gpSkinMeshThreadCan){
        gpSkinMeshThreadCan->stop();
        delete gpSkinMeshThreadCan;
    }
    if(gpSkinMeshThreadPort){
        gpSkinMeshThreadPort->stop();
        delete gpSkinMeshThreadPort;
    }

    mutex.unlock();
}


void QtICubSkinGuiPlugin::paint(QPainter *painter)
{

    if(!timer.isActive()){
        return;
    }

    int width=painter->device()->width();
    int height=painter->device()->height();


   bDrawing=false;

   if (bDrawing){
       return;
   }

   bDrawing=true;

   mutex.lock();
    if (width!=gWidth || height!=gHeight){
        gWidth=width;
        gHeight=height;

        gRowStride=3*gWidth;
        gImageSize=gRowStride*gHeight;
        gImageArea=gWidth*gHeight;
        gMapSize=gWidth*gHeight*sizeof(double);

        if (gpActivationMap){
            delete [] gpActivationMap;
        }
        if (gpImageBuff){
            delete [] gpImageBuff;
        }
        gpActivationMap = new double[gImageArea];
        gpImageBuff = new uchar[gImageSize];

        if(TheadType == TYPE_CAN && gpSkinMeshThreadCan  && gWidth>=180 && gHeight>=180){
            gpSkinMeshThreadCan->resize(gWidth,gHeight);
        } else if (TheadType == TYPE_PORT && gpSkinMeshThreadPort && gWidth>=180 && gHeight>=180){
            gpSkinMeshThreadPort->resize(gWidth,gHeight);
        }
    }

    if (TheadType == TYPE_CAN && gpSkinMeshThreadCan){
        memset(gpImageBuff,0,gImageSize);

        if (gWidth>=180 && gHeight>=180){
            gpSkinMeshThreadCan->eval(gpImageBuff);
            gpSkinMeshThreadCan->draw(gpImageBuff);
        }

        QImage img = QImage(gpImageBuff,gWidth,gHeight,gRowStride,QImage::Format_RGB32);
        painter->beginNativePainting();
        painter->fillRect(0,0,gWidth,gHeight,QColor("black"));
        painter->drawImage(0,0,img);
        painter->endNativePainting();

    }else if (TheadType == TYPE_PORT && gpSkinMeshThreadPort) {
        memset(gpImageBuff,0,gImageSize);

        if (gWidth>=180 && gHeight>=180)
        {
            gpSkinMeshThreadPort->eval(gpImageBuff);
            gpSkinMeshThreadPort->draw(gpImageBuff);
        }


        QImage img = QImage(gpImageBuff,gWidth,gHeight,gRowStride,QImage::Format_RGB888);

        painter->beginNativePainting();
        painter->fillRect(0,0,gWidth,gHeight,QColor("black"));
        painter->drawImage(0,0,img);
        painter->endNativePainting();
    }

    mutex.unlock();
    bDrawing=false;

}


/*! \brief parse the parameters received from the main container in QstringList form
    \param params the parameter list
*/
bool QtICubSkinGuiPlugin::parseParameters(QStringList params)
{
    Network yarp;

    rf.setDefaultContext("skinGui/skinGui");
    rf.setDefaultConfigFile("left_hand.ini");

    // Transform Qt Params array in standard argc & argv
    int c = params.count();
    char **v;
    v = (char**)malloc(sizeof(char*) * c);
    for(int i=0;i<params.count();i++){
        v[i] = (char*)malloc(sizeof(char) * params.at(i).length()+1);
        strcpy(v[i],params.at(i).toLatin1().data());
    }

    if(!rf.configure(c, v)){
        free(v);
        return false;
    }

    gWidth =rf.find("width" ).asInt();
    gHeight=rf.find("height").asInt();
    if (rf.check("xpos")){
        gXpos=rf.find("xpos").asInt();
    }
    if (rf.check("ypos")){
        gYpos=rf.find("ypos").asInt();
    }

    bool useCan = rf.check("useCan");
    if (useCan==true){
        yInfo("CAN version: Reading data directly from CAN");
        TheadType=TYPE_CAN;
    } else {
        yInfo("YARP version: reading data from a Yarp port");
        TheadType=TYPE_PORT;
    }


    window_title=QString("SkinGui (part: %1)").arg(rf.find("robotPart").asString().data());
    windowTitleChanged();
    posXChanged();
    posYChanged();
    widthChanged();
    heightChanged();

    yDebug("RF: %s",rf.toString().data());

    gRowStride=3*gWidth;
    gImageSize=gRowStride*gHeight;
    gMapSize=gWidth*gHeight*sizeof(double);
    gImageArea=gWidth*gHeight;

    gpActivationMap=new double[gImageArea];
    gpImageBuff=new uchar[gImageSize];

    /***********/
    // TODO
    /***********/

    timer.start();

    connect(this,SIGNAL(init()),this,SLOT(onInit()),Qt::QueuedConnection);
    init();


//    if (gpActivationMap) delete [] gpActivationMap;
//    if (gpImageBuff) delete [] gpImageBuff;



    // TODO DESTROY

    return true;
}


void QtICubSkinGuiPlugin::onTimeout()
{
    update();
}

void QtICubSkinGuiPlugin::onInit()
{
    int period = rf.check("period")?rf.find("period").asInt():50;

    if (TheadType==TYPE_CAN)
    {
        gpSkinMeshThreadCan=new SkinMeshThreadCan(rf,period);
        gpSkinMeshThreadCan->start();

    }
    else if (TheadType==TYPE_PORT)
    {
        gpSkinMeshThreadPort=new SkinMeshThreadPort(rf,period);
        gpSkinMeshThreadPort->start();
    }

    done();
}

/*! \brief Returns the title.*/
QString QtICubSkinGuiPlugin::windowTitle()
{
    return window_title;
}

/*! \brief Returns the x position.*/
int QtICubSkinGuiPlugin::posX()
{
    return gXpos;
}

/*! \brief Returns the y position.*/
int QtICubSkinGuiPlugin::posY()
{
    return gYpos;
}

/*! \brief Returns the width.*/
int QtICubSkinGuiPlugin::windowWidth()
{
    return gWidth;
}

/*! \brief Returns the height.*/
int QtICubSkinGuiPlugin::windowHeight()
{
    return gHeight;
}
